﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace Hook.Primitives
{
    public class Matrix3x3
    {
        public float[,] Matrix = new float[3, 3];

        public Matrix3x3()
            : this(0)
        {
        }

        public Matrix3x3(float Value)
        {
            for (int r = 0; r < Matrix.GetLength(0); r++)
            {
                for (int c = 0; c < Matrix.GetLength(1); c++)
                {
                    if(r == c)
                        Matrix[r, c] = Value;
                    else
                        Matrix[r, c] = 0;
                }
            }
        }

        public Matrix3x3(Vector3 Value)
        {
            Matrix[0, 0] = 0;           Matrix[0, 1] = -Value.Z;    Matrix[0, 2] = Value.Y;
            Matrix[1, 0] = Value.Z;     Matrix[1, 1] = 0;           Matrix[1, 2] = -Value.X;
            Matrix[2, 0] = -Value.Y;    Matrix[2, 1] = Value.X;     Matrix[2, 2] = 0;
        }

        public float Determinant()
        {
            float RightDiagonal = Matrix[0, 0] * Matrix[1, 1] * Matrix[2, 2] + Matrix[0, 1] * Matrix[1, 2] * Matrix[2, 0] + Matrix[0, 2] * Matrix[1, 0] * Matrix[2, 1];
            float LeftDiagonal = Matrix[0, 1] * Matrix[1, 0] * Matrix[2, 2] + Matrix[0, 0] * Matrix[1, 2] * Matrix[2, 1] + Matrix[0, 2] * Matrix[1, 1] * Matrix[2, 0];

            return RightDiagonal - LeftDiagonal;
        }

        public Matrix3x3 Inverse()
        {
            float Det = Determinant();
            if(Det == 0)
            {
                return null;
            }
            else
            {
                Matrix3x3 Inverse = new Matrix3x3();

                Inverse.Matrix[0, 0] = (Matrix[2, 2] * Matrix[1, 1] - Matrix[2, 1] * Matrix[1, 2]) / Det;
                Inverse.Matrix[0, 1] = (Matrix[2, 1] * Matrix[0, 2] - Matrix[2, 2] * Matrix[0, 1]) / Det;
                Inverse.Matrix[0, 2] = (Matrix[1, 2] * Matrix[0, 1] - Matrix[1, 1] * Matrix[0, 2]) / Det;

                Inverse.Matrix[1, 0] = (Matrix[2, 0] * Matrix[1, 2] - Matrix[2, 2] * Matrix[1, 0]) / Det;
                Inverse.Matrix[1, 1] = (Matrix[2, 2] * Matrix[0, 0] - Matrix[2, 0] * Matrix[0, 2]) / Det;
                Inverse.Matrix[1, 2] = (Matrix[1, 0] * Matrix[0, 2] - Matrix[1, 2] * Matrix[0, 0]) / Det;

                Inverse.Matrix[2, 0] = (Matrix[2, 1] * Matrix[1, 0] - Matrix[2, 0] * Matrix[1, 1]) / Det;
                Inverse.Matrix[2, 1] = (Matrix[2, 0] * Matrix[0, 1] - Matrix[2, 1] * Matrix[0, 0]) / Det;
                Inverse.Matrix[2, 2] = (Matrix[1, 1] * Matrix[0, 0] - Matrix[1, 0] * Matrix[0, 1]) / Det;

                return Inverse;
            }
        }

        public Matrix3x3 SimpleInverse()
        {
            Matrix3x3 InverseMatrix = new Matrix3x3(1);
            for (int n = 0; n < Matrix.GetLength(0); n++)
            {
                InverseMatrix.Matrix[n, n] = 1 / Matrix[n, n];
            }
            return InverseMatrix;
        }

        public Matrix3x3 Multiply(Matrix3x3 Mult)
        {
            Matrix3x3 ProductMatrix = new Matrix3x3();

            for (int r = 0; r < Matrix.GetLength(0); r++)
            {
                for (int c = 0; c < Matrix.GetLength(1); c++)
                {
                    for (int n = 0; n < Mult.Matrix.GetLength(0); n++)
                    {
                        ProductMatrix.Matrix[r, c] += Matrix[r, n] * Mult.Matrix[n, c];
                    }
                }
            }
            return ProductMatrix;
        }

        public Vector3 Multiply(Vector3 Mult)
        {
            Vector3 ProductVector = new Vector3();

            if (Mult.X == 0)
                ProductVector.X = 0;
            else
                ProductVector.X = Matrix[0, 0] * Mult.X + Matrix[0, 1] * Mult.Y + Matrix[0, 2] * Mult.Z;
            if (Mult.Y == 0)
                ProductVector.Y = 0;
            else
                ProductVector.Y = Matrix[1, 0] * Mult.X + Matrix[1, 1] * Mult.Y + Matrix[1, 2] * Mult.Z;
            if (Mult.Z == 0)
                ProductVector.Z = 0;
            else
                ProductVector.Z = Matrix[2, 0] * Mult.X + Matrix[2, 1] * Mult.Y + Matrix[2, 2] * Mult.Z;

            return ProductVector;
        }

        public Matrix3x3 Multiply(float Mult)
        {
            Matrix3x3 ProductMatrix = new Matrix3x3();

            for (int r = 0; r < Matrix.GetLength(0); r++)
            {
                for (int c = 0; c < Matrix.GetLength(1); c++)
                {
                    ProductMatrix.Matrix[r, c] = Mult * Matrix[r, c];
                }
            }
            return ProductMatrix;
        }

        public Matrix3x3 Add(Matrix3x3 Add)
        {
            Matrix3x3 SumMatrix = new Matrix3x3();

            for (int r = 0; r < Matrix.GetLength(0); r++)
            {
                for (int c = 0; c < Matrix.GetLength(1); c++)
                {
                    SumMatrix.Matrix[r, c] = Matrix[r, c] + Add.Matrix[r, c];
                }
            }
            return SumMatrix;
        }

        public Matrix3x3 Subtract(Matrix3x3 Sub)
        {
            Matrix3x3 DifferenceMatrix = new Matrix3x3();

            for (int r = 0; r < Matrix.GetLength(0); r++)
            {
                for (int c = 0; c < Matrix.GetLength(1); c++)
                {
                    DifferenceMatrix.Matrix[r, c] = Matrix[r, c] - Sub.Matrix[r, c];
                }
            }
            return DifferenceMatrix;
        }
    }
}
